package edu.tec.mty.extras;

import java.util.TreeMap;

import edu.tec.mty.core.modules.Self;
import edu.tec.mty.core.modules.models.Position;

/**
 * MotionPrediction Class, It predicts position of the player based on current or previous commands, 
 * making more accurate the current location
 * 
 * @author Luis Carlos González Sua
 */

public class MotionPrediction {
	public double currentSpd;					//Current Speed of the Player
	public boolean predictOk;					//Prediction verification flag
	public double playerEffort = Self.EFFORT;			//Effort from server
	public double dpr = Self.DASH_POWER_RATE;					//Dash Power Rate
	public double spdDecay = Self.PLAYER_DECAY;				//Speed Decay of the Player
	public double inertiaMoment = Self.INERTIA_MOMENT;
	public double initialPos = 0;				//Initial Position
	public double finalPos;						//Final Position
	public double accel;						//Acceleration per cycle
	public double dashPower;					//Power of dash
	public double delay = 1;					//Transmission delay
	private int limit = 10;						//Buffer size limit
	public double errorMargin = 0.15;			//Margin of error for predictions 
	private TreeMap<Long, Double> dMap = new TreeMap<Long, Double>(); // Buffer variable dash
	private TreeMap<Long, Double> tMap = new TreeMap<Long, Double>(); // Buffer variable turn
	private TreeMap<Long, Double> sMap = new TreeMap<Long, Double>(); // Buffer variable Speed
	private TreeMap<Long, Double> hMap = new TreeMap<Long, Double>(); // Buffer variable Direction

	/**
	 * Distance to Stop Calculates current distance to stop based on previous dash an
	 * 
	 * @param dPower
	 * @param currentSpeed
	 * @return finalPos
	 */
	public double distanceToStop(double dPower, double pSpd){

		dashPower = dPower;
		delay = 1;
		finalPos = 0;
		initialPos = 0;
		while (pSpd > 0.01){
			accel = pSpd + playerEffort * dpr * dashPower;
			finalPos = initialPos + accel;
			pSpd = spdDecay * accel;
			initialPos = finalPos;	
			if (delay == 0){
				dashPower = 0;
			}
			else{
				delay--;
			}
		}

		return finalPos;
	}
	
	public Position getNewPos(double actualX, double actualY, double actualSpd, double actualDirSpd, double dashExecuted){
		accel = actualSpd + playerEffort * dpr * dashExecuted;
		double newX = actualX + accel * Math.cos(Math.toRadians(actualDirSpd));
		double newY = actualY + accel * Math.sin(Math.toRadians(actualDirSpd));
		
		return new Position(newX, newY, actualDirSpd);
	}
	
	public double getNewDirection (double actualAngle, double actualSpd, double turnExecuted){
		return actualAngle + (turnExecuted / (1.0 + Self.INERTIA_MOMENT * actualSpd));
	}
	
	public double getNewSpd(double actualSpd, double dashExecuted){
		accel = actualSpd + playerEffort * dpr * dashExecuted;
		return spdDecay * accel;
	}

	public synchronized void addData(long time, double dash, double turn, 
			double speed, double heading) {
		if (dMap.size() < limit) {
			dMap.put(time, dash);
		} else {
			long older = dMap.firstKey();
			dMap.remove(older);
			dMap.put(time, dash);
		}
		
		if (tMap.size() < limit) {
			tMap.put(time, turn);
		} else {
			long older = tMap.firstKey();
			tMap.remove(older);
			tMap.put(time, turn);
		}
		
		if (sMap.size() < limit) {
			sMap.put(time, speed);
		} else {
			long older = sMap.firstKey();
			sMap.remove(older);
			sMap.put(time, speed);
		}
		
		if (hMap.size() < limit) {
			hMap.put(time, heading);
		} else {
			long older = hMap.firstKey();
			hMap.remove(older);
			hMap.put(time, heading);
		}
	}

	public synchronized double getDashAt(long time) {
		return dMap.get(time);
	}
	
	public synchronized double getTurnAt(long time) {
		return tMap.get(time);
	}
	
	public synchronized double getSpeedAt(long time) {
		return sMap.get(time);
	}
	
	public boolean verifyPredict(Position predicted, Position actual){
		if((predicted.getX() <= actual.getX() + errorMargin)&&
		   (predicted.getX() >= actual.getX() - errorMargin)&&
		   (predicted.getY() <= actual.getY() + errorMargin)&& 
		   (predicted.getY() >= actual.getY() - errorMargin)){
			predictOk = true;
		} else{
			predictOk = false;
		}
		return predictOk;
	}
}
